#include "./../inc/matrix.h"

#include <string.h>
#include <stdio.h>
#include <memory.h>
#include <stdlib.h>
#include <math.h>
enum {
	ROWS = 4,
	COLUMNS = 4,
	ELEMENTS = 16
};

int ufoIndentityMatrix(ufoMatrix4 *matrix)
{
	if (matrix) {
		memset(matrix, 0, sizeof(ufoMatrix4));
		matrix->e00 = matrix->e11 = matrix->e22 = matrix->e33 = 1.0f;
		return 1;
	}
	return 0;
}

float ufoMatrixGetElement(ufoMatrix4 *matrix, int row, int column)
{
	if (matrix) {
		return matrix->mElements[row+column*ROWS];
	}
	return 0;
}

int ufoMatrixSetElement(ufoMatrix4 *matrix, int row, int column, float v)
{
	if (matrix) {
		matrix->mElements[row+column*ROWS] = v;
		return 1;
	}
	return 0;
}

int ufoMultMatrix(ufoMatrix4 *output, ufoMatrix4 *in1, ufoMatrix4 *in2)
{
	int i,j,k;
	float fSum;
	ufoMatrix4 _in1 = *in1;
	ufoMatrix4 _in2 = *in2;
	for (i = 0; i<4; ++i)	
	{
		for (j = 0; j<4; ++j)		
		{
			fSum = 0.0f;
			for (k = 0; k<4; ++k)	 {
				fSum += (ufoMatrixGetElement(&_in1, i, k) * 
					ufoMatrixGetElement(&_in2, k, j));//a.ElementC(i,k) * b.ElementC(k,j);
			}
			//result.Element(i,j) = fSum;
			ufoMatrixSetElement(output, i, j, fSum);
		}
	}
	return 0;
}

int ufoTranslateMatrix(ufoMatrix4 *matrix, float x, float y, float z)
{
	if (matrix) {
		ufoIndentityMatrix(matrix);
		matrix->e03 = x;	matrix->e13 = y;	matrix->e23 = z;
		return 1;
	}
	return 0;
}

int ufoScaleMatrix(ufoMatrix4 *matrix, float x, float y, float z)
{
	if (matrix) {
		ufoIndentityMatrix(matrix);
		matrix->e00 = x;	matrix->e11 = y;	matrix->e22 = z;
		return 1;
	}
	return 0;
}

int ufoMatrixRotate(ufoMatrix4 *matrix, float fradian, float x, float y, float z)
{
	if (matrix) {
		float fsin = (float)sin(fradian);
		float fcos = (float)cos(fradian);
		float fone_minus_fcos = 1.0f - fcos;
		ufoIndentityMatrix(matrix);

		matrix->e00 = fcos+fone_minus_fcos*x*x;   matrix->e01 = fone_minus_fcos*x*y-fsin*z; matrix->e02 = fone_minus_fcos*x*z+fsin*y;
		matrix->e10 = fone_minus_fcos*y*x+fsin*z; matrix->e11 = fcos+fone_minus_fcos*y*y;   matrix->e12 = fone_minus_fcos*y*z-fsin*x;
		matrix->e20 = fone_minus_fcos*z*x-fsin*y; matrix->e21 = fone_minus_fcos*z*y+fsin*x; matrix->e22 = fcos+fone_minus_fcos*z*z;
		return 1;
	}
	return 0;
}

int ufoOrthoMatrix(ufoMatrix4 *matrix, float left, float right, float bottom, 
	float top, float nearVal, float farVal)
{
	if (matrix) {
		float oneDivRightSubLeft = 1.0f/(right - left);
		float oneDivTopSubBottom = 1.0f/(top - bottom);
		float oneDivFarSubNear = 1.0f/(farVal - nearVal);

		float tx = -(right + left) * oneDivRightSubLeft;
		float ty = -(top + bottom) * oneDivTopSubBottom;
		float tz = -(farVal + nearVal) * oneDivFarSubNear;

		ufoIndentityMatrix(matrix);
		matrix->e00 = 2 * oneDivRightSubLeft;	matrix->e03 = tx;
		matrix->e11 = 2 * oneDivTopSubBottom;	matrix->e13 = ty;
		matrix->e22 = -2 * oneDivFarSubNear;		matrix->e23 = tz;
		matrix->e33 = 1;
		return 1;
	}
	return 0;
}

int ufoPerspectiveMatrix(ufoMatrix4 *matrix, float fovyInRadian, 
	float aspect, float n, float f)
{
	if (matrix) {
		float s, cot, dz = n - f;
		float rad = 0.5f * fovyInRadian;
		float oneDivDz = 0;
		s = (float)sin(rad);
		if (dz == 0 || s == 0 || aspect == 0) return 0;
		oneDivDz = 1.0f / dz;
		cot = (float)cos(rad)/s;
		ufoIndentityMatrix(matrix);
		matrix->e00 = cot/aspect;
		matrix->e11 = cot;
		matrix->e22 = (f + n) * oneDivDz;
		matrix->e23 = 2.0f * f * n * oneDivDz;
		matrix->e32 = -1.0f;
		matrix->e33 = 0;
		return 1;
	}
	return 0;
}

int ufoMatrixLookAt(ufoMatrix4 *matrix, const ufoVector3 *eye, 
	const ufoVector3 *center, const ufoVector3 *up)
{
	if (matrix && eye && center && up) {
		ufoMatrix4 translateMatrix;
		ufoVector3 rightDir;
		ufoVector3 forwardDir;// = (center - eye).normalizeV();
		ufoVector3 upDir = *up;

		ufoVectorMinus(&forwardDir, center, eye);
		ufoVectorNormalize(&forwardDir);

		ufoVectorNormalize(&upDir);
		
		//= forwardDir * upDir;
		ufoVectorMult(&rightDir, &forwardDir, &upDir);

		ufoVectorMult(&upDir, &rightDir, &forwardDir);
		//upDir = rightDir * forwardDir;

		ufoIndentityMatrix(matrix);
		matrix->e00 = rightDir.x;	matrix->e01 = rightDir.y;	matrix->e02 = rightDir.z;	
		matrix->e10 = upDir.x;		matrix->e11 = upDir.y;		matrix->e12 = upDir.z;		
		matrix->e20 = -forwardDir.x; matrix->e21 = -forwardDir.y;	matrix->e22 = -forwardDir.z;	

		//Matrix4 translateMatrix;
		//MatrixMathUtil::setTranslateMatrix(translateMatrix, -eye.x, -eye.y, -eye.z);

		ufoTranslateMatrix(&translateMatrix, -eye->x, -eye->y, -eye->z);

		//matrix *= translateMatrix;
		ufoMultMatrix(matrix, matrix, &translateMatrix);
		return 1;
	}
	return 0;
}

int ufoVectorMinus(ufoVector3 *vector3, const ufoVector3 *in1, const ufoVector3 *in2)
{
	if (vector3 && in1 && in2) {
		vector3->x = in1->x-in2->x;
		vector3->y = in1->y-in2->y;
		vector3->z = in1->z-in2->z;
		return 1;
	}
	return 0;
}

int ufoVectorMult(ufoVector3 *vector3, const ufoVector3 *a, const ufoVector3 *b)
{
	if (vector3 && a && b) {
		vector3->x = ((a->y * b->z) - (a->z * b->y));													
		vector3->y = ((a->z * b->x) - (a->x * b->z));													
		vector3->z = ((a->x * b->y) - (a->y * b->x));
		return 1;
	}
	return 0;
}

int ufoVectorNormalize(ufoVector3 *vector)
{
	if (vector) {
		float mod = (float)sqrt(vector->x*vector->x + vector->y*vector->y + vector->z*vector->z);
		if(mod != 0.0f)
		{
			float oneDivScalar = 1/mod;
			vector->x *= oneDivScalar;
			vector->y *= oneDivScalar;
			vector->z *= oneDivScalar;
		}	
		return 1;
	}
	return 0;
}